clear all;
close all;
clc;
%% 参数设置
ADC_samples=256;  % 每个chirp ADC采样点数
Nfft1=256;        % 1D FFT Num
Nchirp=128;       % 每帧chirp数,应该是*3*4
Nframe=32;      % 帧数
NTx=2;           % 发射天线数
NRx=4;           % 接收天线数
C=3e8;           % 光速
sampleRate=5e6 ;  % ADC采样率
startFreq =77e9;  % 雷达发射信号起始频率
Nfft2=Nchirp;        % MIMO后相当于每个天线loop的次数
FocusFrameNum=Nframe;% 观看帧数

%波束参数 
lambda=C/startFreq;             % 发射信号波长
d=lambda/2;                     % 天线间距
BeamWidth=1.22*lambda/(NRx*d);  % 波束宽度

%推算参数
Tchirp_ramp=60e-6;              % chirp 扫频时间 
Tchirp=(Tchirp_ramp+30e-6)*NTx;   % 循环chirp总时间  TDM-MIMO模式（2/3发）

Tframe_set=100e-3;               % 帧周期 55ms
B_set=1.799e9;                  % 扫频带宽

B= B_set*ADC_samples/Tchirp_ramp/sampleRate;           % 发射信号有效带宽 
freqSlope=54.725e12;%B_set/Tchirp_ramp;                % 调频斜率

win1=repmat(hamming(ADC_samples),1,Nfft2*NTx*NRx);     %for 1D FFT 窗长
win2=repmat(hamming(Nchirp).',ADC_samples,1);          %for 2D FFT 窗长

FileName = 'adc_data_ycheren.bin'; 
 adc_data = readDCA1000(FileName,ADC_samples,NRx,NTx);                                          %读取文件
%% 坐标
[X1,Y1] = meshgrid(C*(0:ADC_samples-1)*sampleRate/2/freqSlope/ADC_samples, ...   %坐标计算 转换为真实目标的距离和速度
                (-Nchirp/2:Nchirp/2 - 1)*lambda/Tchirp/Nchirp/2);   
%% 跟踪算法的参数
global Tracker
global RADARDEMO_CT_MAX_NUM_CLUSTER
global RADARDEMO_CT_MAX_NUM_TRACKER
global RADARDEMO_CT_MAX_NUM_ASSOC
global RADARDEMO_CT_MAX_DIST
global RADARMEDO_CT_MAX_NUM_EXPIRE
RADARDEMO_CT_MAX_NUM_CLUSTER=800; %最大扫描簇数
RADARDEMO_CT_MAX_NUM_TRACKER=30;  %最大跟踪数
RADARDEMO_CT_MAX_NUM_ASSOC=6;     %最大可关联数？簇数？一个最多关联6个？？
RADARDEMO_CT_MAX_DIST=0.1e8;      %距离
RADARMEDO_CT_MAX_NUM_EXPIRE=16;   %过期数目
global activeTrackerList
global idleTrackerList
dbscan_nAccFrames=1;
dt=Tframe_set * dbscan_nAccFrames;

%RADARDEMO_clusterTracker_updateFQ
% 至于状态转移矩阵为啥是这样，还是和S_hat有关系的，S_hat是[x,y,vx,vy]
% x是距离，他的变化和vx有关成dt倍
% y类似
F =        [1, 0, dt, 0; 
            0, 1, 0,  dt;
            0, 0, 1,  0; 
            0, 0, 0,  1];    
c =  (dt*dt*4.0); % (dt*2)^2 *d，
b =  (c*dt*2);    % (dt*2)^3 *d
a =  (c*c);       % (dt*2)^4 *d
% 至于协方差矩阵为啥是这样？
% 取1、3或者2、4会得到[a,b;b,c]小矩阵

Q = [a, 0, b, 0;
     0, a, 0, b;
     b, 0, c, 0;
     0, b, 0, c];
clear a b c 
activeTrackerList=[];
idleTrackerList=1:RADARDEMO_CT_MAX_NUM_TRACKER;
Tracker.S_apriori_hat=ones(4,1);   %先验？先前？可能是预测的。<状态>预测值
Tracker.S_hat=ones(4,1);           %<状态>预测值
Tracker.H_s_apriori_hat=ones(3,1); %转换成了球面
Tracker.diagonal2=4;
Tracker.state=-1;
Tracker.detect2freeCoun=3;
Tracker.detect2activeCount=1;
Tracker.detect2freeCount=3;
Tracker.active2freeCount=6;
Tracker.typeCount = 0;
measurementNoiseVariance   = 1;     %1
iirForgetFactor   = 1;              %0.2
activeThreshold   = 6;              %2
forgetThreshold   = 4;              %4
%跟踪门限
Track_Thres=[0.1 1 0.1];   %r_Th  x_Th  v_Th
MmwDemo_Tracking_s_count =0;%跟踪目标计数器

%% 帧循环处理数据
for FrameIndx= 42:47%20:46
    datain =adc_data(:,(( FrameIndx-1 )*Nfft1*Nfft2 + 1 ):( Nfft1*Nfft2*FrameIndx ));
    datain=datain.';   
    SigReshape=reshape(datain,ADC_samples,Nfft2*NTx*NRx);

%% 2DFFT
    [Sig_fft2D]=RangeDopplerProcessing(SigReshape,ADC_samples,Nfft2,NTx,NRx,win1,win2); %win的参数 重新计算
    %调试   

    detMatrix = zeros(Nfft1,Nchirp);
    for i = 1 : NTx * NRx
        detMatrix = detMatrix + reshape(abs(Sig_fft2D(:,:,i)),Nfft1,Nchirp);
    end 

%% 多普勒维度进行CFAR
dopplerDimCfarThresholdMap = zeros(size(detMatrix));  %创建一个二维矩阵存放doppler维cfar后的结果
dopplerDimCfarResultMap = zeros(size(detMatrix));

% %保护单元：
% %虚警概率：0.012,PFAv（门限）越大，pfa越小
% %门限值：
%cfar
Tv=12;Pv=8;PFAv=5;%PFAv=5;
Tr=8;Pr=4; PFAr=2;%3;%PFAr=8;

for i = 1:ADC_samples
    dopplerDim = reshape(detMatrix(i,:),1,Nchirp);  %变成一行数据
    %返回结果和门槛
    [cfar1D_Arr,threshold] = ca_cfar(Tv,Pv,PFAv,dopplerDim);  %进行1D cfar
    dopplerDimCfarResultMap(i,:) = cfar1D_Arr; 
    dopplerDimCfarThresholdMap(i,:) = threshold;    %速度维 cfar 的门槛
end
%%
%沿着doppler维度方向寻找在doppler维cfar判决后为1的结果
saveMat = zeros(size(detMatrix));
for range = 1:ADC_samples
    indexArr = find(dopplerDimCfarResultMap(range,:)==1)';
    
    objDopplerArr = [indexArr;zeros(Nchirp - length(indexArr),1)];   %补充长度
    
    saveMat(range,:) = objDopplerArr;               %保存doppler下标
end
                                                    % 保存有物体的doppler坐标
objDopplerIndex = unique(saveMat);                  % unqiue是不重复的返回数组中的数
                                                    % 根据之前doppler维的cfar结果对应的下标saveMat，对相应的速度进行range维度的CFAR
                                                    
rangeDimCfarThresholdMap = zeros(size(detMatrix));  %创建一个二维矩阵存放range维cfar后的结果
rangeDimCfarResultMap = zeros(size(detMatrix));
outSNRDimCfarThresholdMap= zeros(size(detMatrix));

i = 1;
while(i<=length(objDopplerIndex))
    if(objDopplerIndex(i)==0)                                            
        i = i + 1;
        continue;
    else                                                                
        j = objDopplerIndex(i); 
        
        rangeDim = reshape(detMatrix(:,j),1,ADC_samples);                
        
        [cfar2D_Arr,threshold,outSNR] = ca_cfar2d(Tr,Pr,PFAr,rangeDim);  
        rangeDimCfarResultMap(:,j) = cfar2D_Arr; 
        rangeDimCfarThresholdMap(:,j) = threshold;
        outSNRDimCfarThresholdMap(:,j) =outSNR;
        i = i + 1;  
    end
end
    %调试
%     figure(3);
%     mesh(X1,Y1,(rangeDimCfarResultMap)');
%     title(num2str(FrameIndx));
%     xlabel('距离');
%     ylabel('速度');
%     zlabel('幅度');
%     view(2);

%索引值 
[objRagIdx,objDprIdx] = peakFocus(rangeDimCfarResultMap,detMatrix);
objDprIdx(objDprIdx==0)=[]; 
objRagIdx(objRagIdx==0)=[];
outSNRDimCfarThresholdMap(find(outSNRDimCfarThresholdMap==0))=[];
objSpeed = ( objDprIdx - Nchirp/2 - 1)*lambda/Tchirp/Nchirp/2;

Sig_fft2D1 = zeros(Nfft1,Nfft2,NTx*NRx);

%% 多普勒相位补偿
for k =1:NTx %这里将3改为了NTx
    for  m=1:Nfft1
         Sig_fft2D1(m,:,((k-1)*4+1):k*4) =   Sig_fft2D(m,:,((k-1)*4+1):k*4)*exp(-j*2*3.14*k*m/(NTx*Nfft2));
    end
end
Sig_fft2D  =Sig_fft2D1;
%% 求角度
objRange = single(C*(objRagIdx-1)*sampleRate/2/freqSlope/ADC_samples);
temp_sig_fft2D=Sig_fft2D(:,:,1:8);
if(~isempty(objDprIdx))
    angle =processingChain_angleFFT(objDprIdx,objRagIdx,temp_sig_fft2D);%music
    angle = (angle+90)'./57.8;
else
end

%% 信息整合
if(~isempty(objDprIdx))

    Eangle = 0;
    h = objRange.*sin(Eangle);
    snr = outSNRDimCfarThresholdMap'; %数据整合
    X = objRange.*cos(angle);
    Y = objRange.*sin(angle);
    RVA = [ Y,X,h ,objSpeed,snr];     %散射点的信息集合

%% 聚类DBSCAN与跟踪
    % 聚类参数
    eps = 1.1;       %邻域半径
    minPointsInCluster = 9;
    xFactor = 1;   %变大控制距离变大，变小分类距离变小 椭圆
    yFactor = 1;   %变大控制角度变大，变小分类距离变小 椭圆 

    %画坐标区
    figure(4)
    write_jizuobiao; 
    sumClu =DBSCANclustering_eps_test_3D(eps,RVA,xFactor,yFactor,minPointsInCluster,FrameIndx);  %2D聚类

    %  跟踪算法
    MmwDemo_Tracking_s(sumClu,F,Q,measurementNoiseVariance,activeThreshold,forgetThreshold,...%扩展的卡尔曼滤波
                         iirForgetFactor,...
                         Track_Thres);
end  
%% 跟踪图形绘制
if ~isempty(length(sumClu))
    for clength=1:length(sumClu)
         xx=sumClu(clength).y;
         yy=sumClu(clength).x; 
         vv=sumClu(clength).v;
         hold on;
         
         if vv >=2
             plot(xx,yy,'bs','MarkerSize',65)
             text(xx+0.75,yy,["分类: 车",]);
             text(xx+0.75,yy+0.3,['速度:',num2str(sumClu(clength).v),'  m/s']); %速度
         
         else
             plot(xx,yy,'bd','MarkerSize',25)
             text(xx+0.5,yy,["分类: 人",]);
             text(xx+0.5,yy+0.3,['速度:',num2str(sumClu(clength).v),'  m/s']); %速度
         end
    end

end

%% 清除图片
pause(0.2);
hold off
RVA=[];

end


                                            
